#include "global.h"

// 定义圆周率常量
#define IMU_PI 3.14159265358979323846

static float imuMadgwickBeta = 0.2f; // Madgwick算法参数，用于调整滤波器增益

void madgwick_update(_st_Mpu_F *pMpu_F, _st_Quaternion *pQ, float beta, float dt)
{
    // 将陀螺仪数据从度/秒转换为弧度/秒
    float gx = pMpu_F->gyroX * (float)(IMU_PI / 180.0f);
    float gy = pMpu_F->gyroY * (float)(IMU_PI / 180.0f);
    float gz = pMpu_F->gyroZ * (float)(IMU_PI / 180.0f);

    // 提取加速度计数据
    float ax = pMpu_F->accX;
    float ay = pMpu_F->accY;
    float az = pMpu_F->accZ;

    // 归一化加速度计测量值，确保其在单位球面上
    float norm = sqrtf(ax * ax + ay * ay + az * az);
    if (norm == 0.0f)
    {
        return; // 处理无效数据（避免除以零）
    }
    ax /= norm;
    ay /= norm;
    az /= norm;

    // 计算预测的重力方向（基于当前四元数）
    float vx = 2.0f * (pQ->q1 * pQ->q3 - pQ->q0 * pQ->q2);
    float vy = 2.0f * (pQ->q0 * pQ->q1 + pQ->q2 * pQ->q3);
    float vz = pQ->q0 * pQ->q0 - pQ->q1 * pQ->q1 - pQ->q2 * pQ->q2 + pQ->q3 * pQ->q3;

    // 计算误差向量（实际重力方向与预测重力方向的偏差）
    float ex = (ay * vz - az * vy);
    float ey = (az * vx - ax * vz);
    float ez = (ax * vy - ay * vx);

    // 根据误差向量调整陀螺仪测量值
    gx += beta * ex;
    gy += beta * ey;
    gz += beta * ez;

    // 计算四元数微分方程
    float qDot1 = 0.5f * (-pQ->q1 * gx - pQ->q2 * gy - pQ->q3 * gz);
    float qDot2 = 0.5f * (pQ->q0 * gx + pQ->q2 * gz - pQ->q3 * gy);
    float qDot3 = 0.5f * (pQ->q0 * gy - pQ->q1 * gz + pQ->q3 * gx);
    float qDot4 = 0.5f * (pQ->q0 * gz + pQ->q1 * gy - pQ->q2 * gx);

    // 积分四元数，更新姿态
    pQ->q0 += qDot1 * dt;
    pQ->q1 += qDot2 * dt;
    pQ->q2 += qDot3 * dt;
    pQ->q3 += qDot4 * dt;

    // 归一化四元数，确保其为单位四元数
    norm = sqrtf(pQ->q0 * pQ->q0 + pQ->q1 * pQ->q1 + pQ->q2 * pQ->q2 + pQ->q3 * pQ->q3);
    if (norm == 0.0f)
    {
        return;
    }
    pQ->q0 /= norm;
    pQ->q1 /= norm;
    pQ->q2 /= norm;
    pQ->q3 /= norm;
}

void eulerToQuaternion(_st_EulerAngle *pEuler, _st_Quaternion *pQ)
{
    // 将角度转换为弧度
    float rollRad = pEuler->roll * ((float)IMU_PI / 180.0f);
    float pitchRad = pEuler->pitch * ((float)IMU_PI / 180.0f);
    float yawRad = pEuler->yaw * ((float)IMU_PI / 180.0f);

    // 计算各角的一半的正弦和余弦值
    float cr = cosf(rollRad * 0.5f);
    float sr = sinf(rollRad * 0.5f);
    float cp = cosf(pitchRad * 0.5f);
    float sp = sinf(pitchRad * 0.5f);
    float cy = cosf(yawRad * 0.5f);
    float sy = sinf(yawRad * 0.5f);

    // 构建四元数
    pQ->q0 = cr * cp * cy + sr * sp * sy;
    pQ->q1 = sr * cp * cy - cr * sp * sy;
    pQ->q2 = cr * sp * cy + sr * cp * sy;
    pQ->q3 = cr * cp * sy - sr * sp * cy;

    // 规范化四元数，确保是单位四元数
    float norm = sqrtf(pQ->q0 * pQ->q0 + pQ->q1 * pQ->q1 + pQ->q2 * pQ->q2 + pQ->q3 * pQ->q3);
    if (norm > 0.0f)
    {
        pQ->q0 /= norm;
        pQ->q1 /= norm;
        pQ->q2 /= norm;
        pQ->q3 /= norm;
    }
}

void quaternionToEuler(_st_Quaternion *pQ, _st_EulerAngle *pEuler)
{
    // 计算滚转角（Roll，X轴）
    float sinr_cosp = 2.0f * (pQ->q0 * pQ->q1 + pQ->q2 * pQ->q3);
    float cosr_cosp = 1.0f - 2.0f * (pQ->q1 * pQ->q1 + pQ->q2 * pQ->q2);
    pEuler->roll = atan2f(sinr_cosp, cosr_cosp);

    // 计算俯仰角（Pitch，Y轴），考虑奇异点（±90°）
    float sinp = 2.0f * (pQ->q0 * pQ->q2 - pQ->q3 * pQ->q1);
    if (fabsf(sinp) >= 1.0f)
    {
        pEuler->pitch = copysignf((float)IMU_PI / 2.0f, sinp); // 奇异点处理
    }
    else
    {
        pEuler->pitch = asinf(sinp);
    }

    // 计算偏航角（Yaw，Z轴）
    float siny_cosp = 2.0f * (pQ->q0 * pQ->q3 + pQ->q1 * pQ->q2);
    float cosy_cosp = 1.0f - 2.0f * (pQ->q2 * pQ->q2 + pQ->q3 * pQ->q3);
    pEuler->yaw = atan2f(siny_cosp, cosy_cosp);

    // 将弧度转换为角度
    pEuler->roll *= (180.0f / (float)IMU_PI);
    pEuler->pitch *= (180.0f / (float)IMU_PI);
    pEuler->yaw *= (180.0f / (float)IMU_PI);
}

void imu_init(float beta)
{
    // 设置Madgwick算法的增益参数
    imuMadgwickBeta = beta;

    // 初始化四元数为单位四元数
    imuQuaternion.q0 = 1.0f;
    imuQuaternion.q1 = 0.0f;
    imuQuaternion.q2 = 0.0f;
    imuQuaternion.q3 = 0.0f;

    // 初始化欧拉角为0
    imuEulerAngle.roll = 0.0f;
    imuEulerAngle.pitch = 0.0f;
    imuEulerAngle.yaw = 0.0f;
}

void imu_getAttitude(void)
{
    // 更新四元数
    madgwick_update(&mpu6050_F, &imuQuaternion, imuMadgwickBeta, dt);

    // 获取欧拉角
    quaternionToEuler(&imuQuaternion, &imuEulerAngle);
}

void imu_resetYaw(void)
{
    imuEulerAngle.yaw = 0.0f;                          // 将偏航角重置为0
    eulerToQuaternion(&imuEulerAngle, &imuQuaternion); // 更新四元数
}
